#include "stdafx.h"

#include "AvatarPhysicsComponent.h"
#include <Physics\Utilities\CharacterControl\CharacterRigidBody\hkpCharacterRigidBody.h>
#include <Physics\Utilities\CharacterControl\FirstPersonCharacter\hkpFirstPersonCharacter.h>
#include <Physics\Utilities\CharacterControl\CharacterRigidBody\hkpCharacterRigidBodyListener.h>
#include "PhysicsSystem.h"
#include <Physics\Collide\Shape\Convex\Capsule\hkpCapsuleShape.h>
#include <Physics\Utilities\CharacterControl\StateMachine\hkpCharacterStateManager.h>
#include <Physics\Utilities\CharacterControl\StateMachine\hkpCharacterState.h>
#include <Physics\Utilities\CharacterControl\StateMachine\InAir\hkpCharacterStateInAir.h>
#include <Physics\Utilities\CharacterControl\StateMachine\Jumping\hkpCharacterStateJumping.h>
#include <Physics\Utilities\CharacterControl\StateMachine\OnGround\hkpCharacterStateOnGround.h>

AvatarPhysicsComponent::AvatarPhysicsComponent(Ogre::Vector3 position) {
	m_time = 0;
	m_currentAngle = HK_REAL_PI * 0.5f;
	m_framesInAir = 0;
	m_updateOrientation = true;

    hkpShape* characterShape;
    {
        hkVector4 bottom( 0.0f, 0.0f, 0.0f );
        hkVector4 top( 0.0f, 2.0f, 0.0f );
        hkReal radius = 0.6f;
        characterShape = new hkpCapsuleShape( bottom, top, radius );
    }

    hkpCharacterRigidBodyCinfo info;
    {
        info.m_shape = characterShape;
        info.m_mass = 100.0f;
        info.m_up.set( 0.0f, 1.0f, 0.0f );
        info.m_position.set( position.x, position.y, position.z);
		//info.m_maxSlope = HK_REAL_PI * 0.33f;
        info.m_supportDistance = 0.1f;
        info.m_hardSupportDistance = 0.01f;
		info.m_maxForce = 1000.0f;
    }

    m_char = new hkpCharacterRigidBody( info );
	m_rigidBody = m_char->getRigidBody();
    hkpCharacterRigidBodyListener* listener = new hkpCharacterRigidBodyListener();
    m_char->setListener( listener );
    listener->removeReference();

	hkpCharacterState* state;
	m_manager = new hkpCharacterStateManager();

	state = new hkpCharacterStateOnGround();
	m_manager->registerState( state, HK_CHARACTER_ON_GROUND);
	state->removeReference();

	state = new hkpCharacterStateInAir();
	m_manager->registerState( state, HK_CHARACTER_IN_AIR);
	state->removeReference();

	state = new hkpCharacterStateJumping();
	m_manager->registerState( state, HK_CHARACTER_JUMPING);
	state->removeReference();

	m_charContext = new hkpCharacterContext(m_manager, HK_CHARACTER_IN_AIR);
	m_charContext->setCharacterType(hkpCharacterContext::HK_CHARACTER_RIGIDBODY);

	//m_charContext->setFilterParameters(1.5f,100.0f,400.0f);

	m_manager->removeReference();

	//Initialize hkpSurfaceInfo of ground from previous frame
	m_previousGround = new hkpSurfaceInfo();
}

void AvatarPhysicsComponent::turn(float angle) {
	m_currentAngle += angle;
}

void AvatarPhysicsComponent::updateCharacter(float timestep, float posX, float posY, bool wantJump) {
	if(m_fresh || timestep <= 0) {
		return;
	}

	{
		//m_currentAngle = 0;
		
		hkVector4 pos = m_char->getRigidBody()->getPosition();
		pos.normalize3();
		hkVector4 rigidBodyNormal = pos;
		hkVector4 upVector = pos;

		if(wantJump) {
			int i = 0;
		}

		m_char->getRigidBody()->getWorld()->markForWrite();
		m_char->getRigidBody()->getWorld()->lock();

		//	Get user input data
		m_currentOrient.setAxisAngle(upVector, m_currentAngle);
		
		//hkQuaternion rot = m_char->getRigidBody()->getRotation();
		//rot.setAxisAngle( upVector, m_currentAngle );
		
		HK_TIMER_BEGIN( "set character state", HK_NULL );

		hkpCharacterInput input;
		hkpCharacterOutput output;
		{
			input.m_inputLR = posX;
			input.m_inputUD = posY;

			input.m_wantJump =  wantJump;
			input.m_atLadder = false;

			input.m_up = upVector;
			input.m_forward.set(1,0,0);
			//input.m_forward = myForwardVector;
			input.m_forward.setRotatedDir( m_currentOrient, input.m_forward );

			hkStepInfo stepInfo;
			stepInfo.m_deltaTime = timestep;
			stepInfo.m_invDeltaTime = 1.0f/timestep;
			
			input.m_stepInfo = stepInfo;

			input.m_characterGravity.set(-upVector(0)*10, -upVector(1)*10, -upVector(2)*10);
			input.m_velocity = m_char->getRigidBody()->getLinearVelocity();
			input.m_position = m_char->getRigidBody()->getPosition();
			
			m_char->checkSupport(stepInfo, input.m_surfaceInfo);

			HK_TIMER_END();
		}

		// Apply the character state machine
		{
			HK_TIMER_BEGIN( "update character state", HK_NULL );

			m_charContext->update( input, output );

			HK_TIMER_END();
		}

		//Apply the player character controller
		{
			HK_TIMER_BEGIN( "simulate character", HK_NULL );

			// Set output velocity from state machine into character rigid body
			m_char->setLinearVelocity(output.m_velocity, timestep);

			HK_TIMER_END();
		}
		

		hkVector4 offset;
		offset.set(1,0,0);
		offset.setRotatedDir( m_currentOrient , offset);

		hkRotation rotation;
		hkVector4& col0 = rotation.getColumn(0);
		hkVector4& col1 = rotation.getColumn(1);
		hkVector4& col2 = rotation.getColumn(2);

		// Smoothed surface normal
		//hkVector4 surfaceNorm;
		//surfaceNorm = ( input.m_surfaceInfo.m_supportedState != hkpSurfaceInfo::UNSUPPORTED ) ? input.m_surfaceInfo.m_surfaceNormal : (hkVector4&)UP;
		//rigidBodyNormal.addMul4( 0.05f, surfaceNorm );
		rigidBodyNormal.normalize3();

		col1 = rigidBodyNormal;
		col2.setCross( col1, offset); 
		col2.normalize3();
		col0.setCross( col1, col2 );

		reorientCharacter( rotation,timestep );

		m_char->getRigidBody()->getWorld()->unlock();
		m_char->getRigidBody()->getWorld()->unmarkForWrite();
	}
}

bool AvatarPhysicsComponent::reorientCharacter(const hkRotation& rotation,hkReal timestep) {
	// Calculate desired rotation velocity to follow expected orientation 
	// angularVel = gain*(desiredOrient - currentOrient)/timestep

	const hkReal gain = 0.5f;
	const hkQuaternion& currentOrient = m_char->getRigidBody()->getRotation();

	hkQuaternion desiredOrient;
	desiredOrient.set(rotation);

	hkVector4 angle;
	currentOrient.estimateAngleTo(desiredOrient,angle);
	
	hkVector4 angularVelocity;
	angularVelocity.setMul4(gain/timestep,angle);

	m_char->setAngularVelocity(angularVelocity);

	return true;
}